#include "LidarDriverPCD.h"
#include <pcl/io/pcd_io.h>
#include <chrono>
#include <thread>

#pragma region Init
LidarDriverPCD::LidarDriverPCD(LidarConfigurationOptions *options, int id)
    : LidarDriverX(options, id)
{
}

void LidarDriverPCD::init()
{
    LidarInputSourceType sourceType = std::get<LidarInputSourceType>(_options->GetValue(LidarConfigurationOptions::Data_Source_SrcType));
    switch (sourceType)
    {
    case LidarInputSourceType::PCD_FILE:
        _pcd_file_path = std::get<std::string>(_options->GetValue(LidarConfigurationOptions::Data_SrcPcd_FilePath));
        break;
    default:
        break;
    }
}

void LidarDriverPCD::dispose()
{
}
#pragma endregion

#pragma region Thread
void LidarDriverPCD::processPointCloud()
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    if (pcl::io::loadPCDFile(_pcd_file_path, *cloud) == -1)
    {
        std::cout << "Couldn't read file " << _pcd_file_path << std::endl;
        _to_exit_process = true;
        return;
    }

    while (!_to_exit_process)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr _cloud_data(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::copyPointCloud(*cloud, *_cloud_data);

        PointCloudData<pcl::PointXYZI> data;
        decodePointCloud(data, _cloud_data);
        data.lidar_id = _lidar_id;

        for (PointCloudDataCallback handler : _pointCloudDataCallback)
        {
            handler(data);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}
#pragma endregion